/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package objectives;

import genotypes.Angles;
import geometry.ConvexHullAlgorithm;
import geometry.IntersectionAlgorithm;
import geometry.Obstacle;
import geometry.Segment;
import geometry.Vertex;
import java.util.Arrays;
import java.util.Iterator;
import java.util.Map.Entry;
import java.util.Set;
import java.util.TreeMap;
import misc.Evaluation;
import misc.ProblemProfile;

/**
 *
 * @author rusak
 */
public class ConvexHullObjective implements ObjectiveFunction {

    private double lengths[];
    private Vertex source, destination;
    private Obstacle obstacles[] = new Obstacle[0];
    private IntersectionAlgorithm intersectionAlgortihm;
    private ConvexHullAlgorithm hullAlgorithm;
    private Obstacle convexHulls[];
    private Obstacle destHull;
    private Segment entranceSegment;
    private Vertex entranceVertex;
    
               
    public ConvexHullObjective(double[] lengths, Vertex source, Vertex destination, Obstacle[] obstacles, IntersectionAlgorithm intersectionAlgortihm, ConvexHullAlgorithm hullAlgorithm) {
        this.lengths = lengths;
        this.source = source;
        this.destination = destination;
        this.obstacles = obstacles;
        this.intersectionAlgortihm = intersectionAlgortihm;
        this.hullAlgorithm = hullAlgorithm;
        convexHulls = new Obstacle[obstacles.length];
        int destHullNumber = -1;
        for (int i = 0; i < obstacles.length; i++) {
            convexHulls[i] = hullAlgorithm.computeHull(obstacles[i]);
          //  //System.out.println("CONVEX HULL COMPUTED: "+convexHulls[i]);
            if (destinationInHull(destination, convexHulls[i])) {
                destHull = convexHulls[i];
                destHullNumber = i;
                
            }
        }
        if (destHull != null) {
            entranceToDestHull(destination, destHull, obstacles[destHullNumber]);
        }
    }
    //ustalamy, ze przeszkody nie maja wiekszego x niz 1000000.

    //tymczasowe
    public ConvexHullObjective(ProblemProfile pp, IntersectionAlgorithm intersectionAlgortihm, ConvexHullAlgorithm hullAlgorithm) {
        this(pp.getLengths(), pp.getSource(), pp.getTarget(), pp.getObstacles(), intersectionAlgortihm, hullAlgorithm);
    }
        
    private boolean destinationInHull(Vertex dest, Obstacle hull) {
        int sidesCount = 0;
        int size = hull.size();
        Segment semiLine = new Segment(dest, new Vertex(1000000, dest.getY()));
        for (int i = 0; i < size; i++) {
            if (hull.getEdgeAt(i).intersect(semiLine)) {
                sidesCount++;
            }
        }
        if (sidesCount % 2 == 1) {
          //  //System.out.println("DEST IN HULL!");
          //  //System.out.println(hull);
            return true;
        } else {
            return false;
        }
    }

    //dopisac to!!
    private void entranceToDestHull(Vertex dest, Obstacle hull, Obstacle obstacle) {
        Segment semiLine;
        Vertex middle, end, begin, inInfinity;
        int size = hull.size();
        for (int i = 0; i < size; i++) {
            begin = hull.getEdgeAt(i).getBegin();
            end = hull.getEdgeAt(i).getEnd();
            middle = new Vertex((begin.getX() + end.getX()) / 2, (begin.getY() + end.getY()) / 2);
            semiLine = new Segment(dest, middle);
            if (semiLine.isVertical()) {
                if (dest.getY() < middle.getY()) {
                    inInfinity = new Vertex(dest.getX(), 1000000);
                } else {
                    inInfinity = new Vertex(dest.getX(), -1000000);
                }
            } else {
                if (dest.getX() < middle.getX()) {
                    inInfinity = new Vertex(1000000, semiLine.getA() * 1000000 + semiLine.getB());
                } else {
                    inInfinity = new Vertex(-1000000, semiLine.getA() * (-1000000) + semiLine.getB());

                }
            }
            semiLine = new Segment(dest, inInfinity);

            int intersectionCount = 0;
            int obstSize = obstacle.size();
            for (int j = 0; j < obstSize; j++) {
                if (obstacle.getEdgeAt(j).intersect(semiLine)) {
                    intersectionCount++;
                }
               
                
            }
             if (intersectionCount == 0) {
                    entranceSegment = hull.getEdgeAt(i);
                    entranceVertex = middle;
                 //   //System.out.println("ENTRANCE SEGMENT: "+entranceSegment);
                 //   //System.out.println("ENTRANCE VERTEX: "+entranceVertex);
                 //   //System.out.println(intersectionCount);
                 //   //System.out.println("SEMI LINE: "+semiLine);
                    return;
                }
          //  //System.out.println("NOT AN ENTRANCE: "+hull.getEdgeAt(i));
          //  //System.out.println(intersectionCount);
          //  //System.out.println("SEMI LINE: "+semiLine);
        }

    }

    
    
    @Override
    public Evaluation evaluate(Angles angles) {
        Segment fromEndToDest[] = new Segment[1];
        Integer intersect = 0;
        if (angles == null) {
            fromEndToDest[0] = new Segment(source, destination);
        } else {
            int n = angles.getAngles().length;
            ////System.out.println("AA"+n);
            Segment segs[] = decode(angles);
            // segs[n] = new Segment(segs[n - 1].getEnd(), destination);


            fromEndToDest[0] = new Segment(segs[n - 1].getEnd(), destination);

            intersect = intersectionAlgortihm.checkIfIntersect(segs, obstacles) ? 1 : 0;
        }
        Integer obstaclesCrossed[] = intersectionAlgortihm.getIntersectedObstacles(fromEndToDest, obstacles);
        Obstacle hullsCrossed[] = new Obstacle[obstaclesCrossed.length];

        for (int i = 0; i < obstaclesCrossed.length; i++) {
              //System.out.println("OBSTACLE NR: "+obstaclesCrossed[i]+" CROSSED!!");
            
            hullsCrossed[i] = convexHulls[obstaclesCrossed[i]];
           //   //System.out.println(hullsCrossed[i]);
        }


        double dist = evalWithConvexHull(fromEndToDest[0], hullsCrossed);
        
        return new Evaluation(dist, intersect, fromEndToDest[0].getLength());

    }

    private double evalWithConvexHull(Segment fromEndToDest, Obstacle convexHulls[]) {
        if (convexHulls.length > 0) {
            double result = 0;
            int size = convexHulls.length;
            //Vertex intersectionPoints[] = new Vertex[convexHulls.length];
            double routes[][] = new double[size][5];

            TreeMap<Double, Vertex> xMap = new TreeMap<Double, Vertex>();
            int coeff = fromEndToDest.getBegin().getX() < fromEndToDest.getEnd().getX()
                    ? 1 : -1;
           ////System.out.println("PREEVALUATION: ");
            for (int i = 0; i < size; i++) {
                routes[i] = routeOnHull(fromEndToDest, convexHulls[i]);
                result += routes[i][0];
                
                if(routes[i][1] == routes[i][3]) {
                    routes[i][3] += coeff*0.000001;
                }
                xMap.put(coeff * routes[i][1], new Vertex(routes[i][1], routes[i][2]));
             //   //System.out.println(coeff * routes[i][1]+" "+new Vertex(routes[i][1], routes[i][2]));
                if (routes[i][3] != -1) {
                    xMap.put(coeff * routes[i][3], new Vertex(routes[i][3], routes[i][4]));
                    //System.out.println(coeff * routes[i][3]+" "+new Vertex(routes[i][3], routes[i][4]));
                }
            }
            Set<Entry<Double, Vertex>> entrySet = xMap.entrySet();
            Iterator<Entry<Double, Vertex>> iter = entrySet.iterator();
            Vertex toPrint = iter.next().getValue();
            result += new Segment(fromEndToDest.getBegin(), toPrint).getLength();
            //System.out.println("EVALUATION:");
            //System.out.println(fromEndToDest.getBegin());
            //System.out.println(toPrint);
            Vertex f, s;
            while (iter.hasNext()) {
                f = iter.next().getValue();
                if (iter.hasNext()) {
                    s = iter.next().getValue();
                } else {
                    s = fromEndToDest.getEnd();
                }
                result += new Segment(f, s).getLength();
                //System.out.println();
                //System.out.println(f);
                //System.out.println(s);
                
            }

            return result;
        } else {
            return fromEndToDest.getLength();
        }
    }

    private double[] routeOnHull(Segment fromEndToDest, Obstacle hull) {
        // //System.out.println("ROUTE ON HULL: "+hull.getEdgeAt(0).getBegin().getX());
        Segment edges[] = hull.getEdges();
        int size = hull.size();
        double rightWay = 0;
        double leftWay = 0;
        boolean right = true;
        Vertex intersection;
        double result[] = new double[5];
        boolean first = true, second = false;
        if (hull == destHull) {
            //System.out.println("ROUTE ON DEST HULL!");
            result[3] = entranceVertex.getX();
            result[4] = entranceVertex.getY();

            for (int i = 0; i < size; i++) {

                if (edges[i] == entranceSegment) {
                    Segment antiClockFirstPart = new Segment(edges[i].getBegin(), entranceVertex);
                    Segment antiClockSecondPart = new Segment(edges[i].getEnd(), entranceVertex);
                    //         //System.out.println("FIRST PART:"+antiClockFirstPart);
                    //         //System.out.println("SECOND PART:"+antiClockSecondPart);
                    //         //System.out.println("GOIN RIGHT? "+right);
                    if (right) {
                        rightWay += antiClockFirstPart.getLength();
                        leftWay += antiClockSecondPart.getLength();
                    }
                    right = !right;
                } else if ((intersection = edges[i].intersectionPoint(fromEndToDest)) != null) {

                    result[1] = intersection.getX();
                    result[2] = intersection.getY();

                    Segment antiClockFirstPart = new Segment(edges[i].getBegin(), intersection);
                    Segment antiClockSecondPart = new Segment(edges[i].getEnd(), intersection);
                    //         //System.out.println("FIRST PART:"+antiClockFirstPart);
                    //         //System.out.println("SECOND PART:"+antiClockSecondPart);
                    //         //System.out.println("GOIN RIGHT? "+right);
                    if (right) {
                        rightWay += antiClockFirstPart.getLength();
                        leftWay += antiClockSecondPart.getLength();
                    } else {
                        rightWay += antiClockSecondPart.getLength();
                        leftWay += antiClockFirstPart.getLength();
                    }
                    right = !right;
                } else {
                    if (right) {
                        //           //System.out.println("TO RIGHT: "+i+" "+edges[i]);

                        rightWay += edges[i].getLength();
                    } else {
                        //         //System.out.println("TO LEFT: "+i+" "+edges[i]);
                        leftWay += edges[i].getLength();
                    }
                }

            }
            result[0] = rightWay < leftWay ? rightWay : leftWay;
            //System.out.println("RESULT: ");
            for(int i=0;i<5;i++) {
                //System.out.print(result[i]+" ");
            }
            //System.out.println();
            return result;
        } else {

            for (int i = 0; i < size; i++) {
                if ((intersection = edges[i].intersectionPoint(fromEndToDest)) != null) {

                    //       //System.out.println("HULL INTERSECTION AT: "+intersection);
                    //      //System.out.println("FIRST: "+first);
                    if (first) {
                        result[1] = intersection.getX();
                        result[2] = intersection.getY();
                        first = false;
                    } else {
                        second = true;
                        result[3] = intersection.getX();
                        result[4] = intersection.getY();
                    }
                    Segment antiClockFirstPart = new Segment(edges[i].getBegin(), intersection);
                    Segment antiClockSecondPart = new Segment(edges[i].getEnd(), intersection);
                    //         //System.out.println("FIRST PART:"+antiClockFirstPart);
                    //         //System.out.println("SECOND PART:"+antiClockSecondPart);
                    //         //System.out.println("GOIN RIGHT? "+right);
                    if (right) {
                        rightWay += antiClockFirstPart.getLength();
                        leftWay += antiClockSecondPart.getLength();
                    } else {
                        rightWay += antiClockSecondPart.getLength();
                        leftWay += antiClockFirstPart.getLength();
                    }
                    right = !right;
                } else {
                    if (right) {
                        //           //System.out.println("TO RIGHT: "+i+" "+edges[i]);

                        rightWay += edges[i].getLength();
                    } else {
                        //         //System.out.println("TO LEFT: "+i+" "+edges[i]);
                        leftWay += edges[i].getLength();
                    }
                }
            }
            if (!second) {
                //zakladamy ze wszytsko dzieje sie w pierwszej cwiartce
                result[3] = -1;
                result[4] = -1;
                result[0] = (rightWay + leftWay) / 2;

            } else {
                result[0] = rightWay < leftWay ? rightWay : leftWay;
            }
            
            return result;
        }
    }

    @Override
    public Segment[] decode(Angles angles) {
        int n = angles.getAngles().length;
        Segment[] segs = new Segment[n];
        segs[0] = new Segment(source, source.traverse(angles.getAngleAt(0), lengths[0]));
        ////System.out.println(segs[0].toString());
        for (int i = 1; i != n; ++i) {
            
            segs[i] = new Segment(segs[i - 1].getEnd(),
                    segs[i - 1].getEnd().traverse(angles.getAngleAt(i), lengths[i]));
        }
        return segs;
    }

}
